//-----------------------------------------------------------------------
//  Form for displaying User Interface
//
//  $File: DriveControl.cs $ $Revision: 1 $
//
//-----------------------------------------------------------------------

using System;
//using System.Collections.Generic;
//using System.ComponentModel;
//using System.Data;
using System.Drawing;
//using System.Text;
using System.Windows.Forms;

// Added references
// Because this is a standard WinForm, it does not automatically get CCR/DSS
using Microsoft.Ccr.Core;
using Microsoft.Dss.Core;
using Microsoft.Dss.ServiceModel.Dssp;

using System.Drawing.Imaging;
using System.Drawing.Drawing2D;
using System.Runtime.InteropServices;
using game = Microsoft.Robotics.Services.GameController.Proxy;
using drive = Microsoft.Robotics.Services.Drive.Proxy;
using webcam = Microsoft.Robotics.Services.WebCam.Proxy;


namespace ProMRDS.Robotics.TeleOperation
{
    public partial class DriveControl : Form
    {
        // This port is passed across to the constructor
        // It allows messages to be sent back to the main service
        DriveControlEvents _eventsPort;
        // This is part of the main service State
        public GUIOptions options;

        #region Initialization and Shutdown

        public DriveControl(DriveControlEvents EventsPort, TeleOperationState state)
        {
            InitializeComponent();

            // Remember the port to use to send back messages
            _eventsPort = EventsPort;

            // Copy the option settings
            options = state.Options;

            txtPort.ValidatingType = typeof(ushort);

            // TT - Add initialization code based on the saved State
            // Set up the connection parameters
            txtHost.Text = state.Host;
            if (state.Port != 0)
                txtPort.Text = state.Port.ToString();

            this.StartPosition = FormStartPosition.Manual;
            this.Location = new Point(options.WindowStartX, options.WindowStartY);

            // The dead zone can't be negative, but it can be zero
            options.DeadZoneX = Math.Abs(state.Options.DeadZoneX);
            options.DeadZoneY = Math.Abs(state.Options.DeadZoneY);

            // Just in case the scale factors have not been initialized
            if (state.Options.TranslateScaleFactor == 0)
                options.TranslateScaleFactor = 1.0;
            else
                options.TranslateScaleFactor = state.Options.TranslateScaleFactor;
            if (state.Options.RotateScaleFactor == 0)
                options.RotateScaleFactor = 0.5;
            else
                options.RotateScaleFactor = state.Options.RotateScaleFactor;
        }

        private void DriveControl_Load(object sender, EventArgs e)
        {
            _eventsPort.Post(new OnLoad(this));
        }

        private void DriveControl_FormClosed(object sender, FormClosedEventArgs e)
        {
            _eventsPort.Post(new OnClosed(this));
        }

        #endregion

        #region Event Handlers

        private void btnConnect_Click(object sender, EventArgs e)
        {
            string host = txtHost.Text;

            if (host.Length == 0)
            {
                MessageBox.Show("Please enter a Host", "Incomplete", MessageBoxButtons.OK, MessageBoxIcon.Warning);
                txtHost.Focus();
                return;
            }

            object obj = txtPort.ValidateText();
            if (obj == null)
                obj = (ushort)0;
            ushort port = (ushort)obj;
            if (port == 0)
            {
                MessageBox.Show("Please enter a Port", "Incomplete", MessageBoxButtons.OK, MessageBoxIcon.Warning);
                txtPort.Focus();
                return;
            }

            if ((port % 10) == 0)
            {
                DialogResult r = MessageBox.Show("Port seems incorrect. Should it be " + (port+1) + "?", "Port Selection", MessageBoxButtons.YesNoCancel, MessageBoxIcon.Question);
                if (r == DialogResult.Yes)
                {
                    port = (ushort)(port + 1);
                    txtPort.Text = port.ToString();
                }
                else if (r == DialogResult.Cancel)
                    return;
            }

            UriBuilder builder = new UriBuilder(Schemes.DsspTcp, host, port, ServicePaths.InstanceDirectory);

            _eventsPort.Post(new OnConnect(this, builder.ToString()));

        }

        private void btnExit_Click(object sender, EventArgs e)
        {
            this.Close();
        }

        private void chkFixedMoves_CheckedChanged(object sender, EventArgs e)
        {
            // Nothing to do
        }

        #endregion

        #region Invoked Methods
        public void ShowErrorMessage(string msg)
        {
            MessageBox.Show(this, msg, "Error", MessageBoxButtons.OK, MessageBoxIcon.Exclamation);
        }

        #endregion

        #region Game Controller

        public void UpdateGameControllerAxes(game.Axes axes)
        {
            int x = axes.X;
            int y = -axes.Y;

            lblX.Text = x.ToString();
            lblY.Text = y.ToString();
            lblZ.Text = axes.Z.ToString();

            DrawJoystick(x, y);

                double left;
                double right;

                //this is the raw length of the vector
                double magnitude = Math.Sqrt(x * x + y * y);

                // Check for the "dead zone"
                // Added some code so that the speed values would
                // not suddenly jump after leaving the Dead Zone
                if (Math.Abs(x) < options.DeadZoneX)
                    x = 0;
                else
                {
                    // Subtract off the dead zone value so that the
                    // coord starts from zero
                    if (x > 0)
                        x -= (int)options.DeadZoneX;
                    else
                        x += (int)options.DeadZoneX;
                }
                if (Math.Abs(y) < options.DeadZoneY)
                    y = 0;
                else
                {
                    if (y > 0)
                        y -= (int)options.DeadZoneY;
                    else
                        y += (int)options.DeadZoneY;
                }

                if (x == 0 && y == 0)
                {
                    // Totally dead in the middle!
                    left = right = 0;
                }
                else
                {
                    //angle of the vector
                    double theta = Math.Atan2(y, x);

                    //this is the maximum magnitude for a given angle
                    //                    double maxMag;
                    double scaledMag = 1.0;
                    //a scaled down magnitude according to above
                    //                    double scaledMag = magnitude * 1000 / maxMag;
                    scaledMag = magnitude;
                    //decompose the vector into motor components
                    // What is the significance of 150? The cross-hairs
                    // cross over at 0, so this just meant that the
                    // cross-over point was slightly below the center
                    // of the yoke ...
                    // NOTE: There is a peculiar problem that if you
                    // try to rotate the robot on the spot, and you
                    // are not careful to keep the cursor on the
                    // horizontal center line, then the robot might
                    // suddenly start rotating in the OPPOSITE direction!
                    // This is a quirk of the maths. It happens if the
                    // cursor moves outside the dead zone on the bottom
                    // (negative) side. This might be the reason for the
                    // -150 in the original code, i.e. to try to avoid
                    // this problem.
                    //if (data.Y > -150)
                    if (y >= 0)
                    {
                        left = scaledMag * options.TranslateScaleFactor * Math.Sin(theta) + scaledMag * options.RotateScaleFactor * Math.Cos(theta);
                        right = scaledMag * options.TranslateScaleFactor * Math.Sin(theta) - scaledMag * options.RotateScaleFactor * Math.Cos(theta);
                    }
                    else
                    {
                        left = scaledMag * options.TranslateScaleFactor * Math.Sin(theta) - scaledMag * options.RotateScaleFactor * Math.Cos(theta);
                        right = scaledMag * options.TranslateScaleFactor * Math.Sin(theta) + scaledMag * options.RotateScaleFactor * Math.Cos(theta);
                    }
                }
                //cap at 1000
                left = Math.Min(left, 1000);
                right = Math.Min(right, 1000);
                left = Math.Max(left, -1000);
                right = Math.Max(right, -1000);
                // Quick and dirty way to display results for debugging -
                // Uncomment the two lines below
                //                Console.WriteLine("Joy: " + data.X + ", " + data.Y
                //                            + " => " + left + ", " + right);
                _eventsPort.Post(new OnMove(this, (int)Math.Round(left), (int)Math.Round(right)));
        }

        public void UpdateGameControllerButtons(game.Buttons buttons)
        {
            if (buttons.Pressed != null && buttons.Pressed.Count > 0)
            {
                string[] buttonString = buttons.Pressed.ConvertAll<string>(
                    delegate(bool button)
                    {
                        return button ? "X" : "O";
                    }
                ).ToArray();

                lblButtons.Text = string.Join(" ", buttonString);

                // Don't do anything with the buttons ...
                // This is up to the reader to implement
                /*
                if (chkStop.Checked && buttons.Pressed.Count > 2)
                {
                    if (buttons.Pressed[2] == true)
                    {
                        chkStop.Checked = false;
                    }
                }
                else if (buttons.Pressed[1] == true && buttons.Pressed.Count > 1)
                {
                    chkStop.Checked = true;
                }

                if (buttons.Pressed[0] != chkDrive.Checked)
                {
                    chkDrive.Checked = buttons.Pressed[0];
                }
                 */
            }

        }

        private void DrawJoystick(int x, int y)
        {
            Bitmap bmp = new Bitmap(picTrackBall.Width, picTrackBall.Height);
            Graphics g = Graphics.FromImage(bmp);

            int width = bmp.Width - 1;
            int height = bmp.Height - 1;

            g.Clear(Color.Transparent);
            g.SmoothingMode = System.Drawing.Drawing2D.SmoothingMode.HighQuality;
            GraphicsPath path = new GraphicsPath();
            path.AddEllipse(0, 0, width, height);

            PathGradientBrush pathBrush = new PathGradientBrush(path);
            pathBrush.CenterPoint = new PointF(width / 3f, height / 3f);
            pathBrush.CenterColor = Color.White;
            pathBrush.SurroundColors = new Color[] { Color.LightGray };

            g.FillPath(pathBrush, path);
            g.DrawPath(Pens.Black, path);

            int partial = y * height / 2200;
            if (partial > 0)
            {
                g.DrawArc(Pens.Black,
                    0,
                    height / 2 - partial,
                    width,
                    2 * partial,
                    180,
                    180);
            }
            else if (partial == 0)
            {
                g.DrawLine(Pens.Black, 0, height / 2, width, height / 2);
            }
            else
            {
                g.DrawArc(Pens.Black,
                    0,
                    height / 2 + partial,
                    width,
                    -2 * partial,
                    0,
                    180);
            }

            partial = x * width / 2200;
            if (partial > 0)
            {
                g.DrawArc(Pens.Black,
                    width / 2 - partial,
                    0,
                    2 * partial,
                    height,
                    270,
                    180);
            }
            else if (partial == 0)
            {
                g.DrawLine(Pens.Black, width / 2, 0, width / 2, height);
            }
            else
            {
                g.DrawArc(Pens.Black,
                    width / 2 + partial,
                    0,
                    -2 * partial,
                    height,
                    90,
                    180);
            }

            picTrackBall.Image = bmp;
        }

        #endregion

        #region TrackBall

        // The "TrackBall" is a simulated trackball on the screen that the
        // user can drag on with the mouse. It fudges game controller updates.
        //
        // Why not use MouseLeave? The Dashboard does this.
        // The problem with MouseLeave is that it is called whenever you move
        // the mouse over the trackball and out again even if you do not want
        // to control the robot. This generates spurious Stop commands.
        //
        // What about MouseDown?
        // Yes, you could use this, but why bother? You still have to track
        // mouse movements.
        //
        // What about the other mouse buttons?
        // Add them yourself in the MouseMove event!

        private void picTrackBall_MouseMove(object sender, MouseEventArgs e)
        {
            // Mouse movements use the Left button only, i.e. drag
            if (e.Button == MouseButtons.Left)
            {
                int x, y;

                // Get cursor coords
                x = Math.Min(picTrackBall.Width, Math.Max(e.X, 0));
                y = Math.Min(picTrackBall.Height, Math.Max(e.Y, 0));

                // Translate to a range of -1000 to +1000
                x = x * 2000 / picTrackBall.Width - 1000;
                y = y * 2000 / picTrackBall.Height - 1000;

                game.Axes axes = new game.Axes();
                axes.X = x;
                axes.Y = y;

                // Fugde a Game Controller update and "send" it
                UpdateGameControllerAxes(axes);
            }
        }

        private void picTrackBall_MouseUp(object sender, MouseEventArgs e)
        {
            // Create empty updates and "send" them
            UpdateGameControllerButtons(new game.Buttons());
            UpdateGameControllerAxes(new game.Axes());
        }

        #endregion

        #region Motion Control Buttons

        // These buttons can use RotateDegrees and DriveDistance which are
        // NOT implemented by all robots. However, you can turn off the
        // Fixed Moves checkbox and then the arrow buttons just turn the
        // motors on as appropriate, sort of like the Robotics Tutorial 4.

        private void btnLeft_Click(object sender, EventArgs e)
        {
            if (chkFixedMoves.Checked)
                _eventsPort.Post(new OnMotionCommand(this, MOTION_COMMANDS.Rotate, options.RotateAngle, options.MotionSpeed));
            else
                TurnLeft();
        }

        private void btnRight_Click(object sender, EventArgs e)
        {
            if (chkFixedMoves.Checked)
                _eventsPort.Post(new OnMotionCommand(this, MOTION_COMMANDS.Rotate, -options.RotateAngle, options.MotionSpeed));
            else
                TurnRight();
        }

        private void btnForward_Click(object sender, EventArgs e)
        {
            if (chkFixedMoves.Checked)
                _eventsPort.Post(new OnMotionCommand(this, MOTION_COMMANDS.Translate, options.DriveDistance / 1000, options.MotionSpeed));
            else
                Forward();
        }

        private void btnStop_Click(object sender, EventArgs e)
        {
            _eventsPort.Post(new OnMotionCommand(this, MOTION_COMMANDS.Stop, 0, 0));
        }

        private void btnReverse_Click(object sender, EventArgs e)
        {
            // TT Oct-2007 - Microsoft says that the correct way to go backwards is to
            // specify a negative power, but the distance should remain positive
            if (chkFixedMoves.Checked)
                _eventsPort.Post(new OnMotionCommand(this, MOTION_COMMANDS.Translate, -options.DriveDistance / 1000, -options.MotionSpeed));
                //_eventsPort.Post(new OnMotionCommand(this, MOTION_COMMANDS.Translate, options.DriveDistance / 1000, -options.MotionSpeed));
            else
                Backward();
        }

        #endregion

        #region Keyboard Handlers

        // NOTE: The arrow keys will not normally appear in a KeyDown event.
        // This is because they will be pre-processed. However, the operation
        // under CF is different and it does no harm to leave them in here.

        private void DriveControl_KeyDown(object sender, KeyEventArgs e)
        {
            switch ((Keys)e.KeyValue)
            {
                case Keys.Up:
                    Forward();
                    e.Handled = true;
                    break;

                case Keys.Down:
                    Backward();
                    e.Handled = true;
                    break;

                case Keys.Left:
                    TurnLeft();
                    e.Handled = true;
                    break;

                case Keys.Right:
                    TurnRight();
                    e.Handled = true;
                    break;

                default:
                    break;
            }

        }

        private void DriveControl_KeyUp(object sender, KeyEventArgs e)
        {
            Stop();
        }

        // Overriding ProcessDialogKey allows us to trap the arrow keys
        protected override bool ProcessDialogKey(Keys keyData)
        {
            switch (keyData)
            {
                case Keys.Up:
                    Forward();
                    return true;

                case Keys.Down:
                    Backward();
                    return true;

                case Keys.Left:
                    TurnLeft();
                    return true;

                case Keys.Right:
                    TurnRight();
                    return true;

                default:
                    break;
            }

            return base.ProcessDialogKey(keyData);
        }

        void Forward()
        {
            _eventsPort.Post(new OnMove(this, (int)options.MotionSpeed, (int)options.MotionSpeed));
        }

        void Backward()
        {
            _eventsPort.Post(new OnMove(this, (int)(-options.MotionSpeed), (int)(-options.MotionSpeed)));
        }

        void TurnLeft()
        {
            _eventsPort.Post(new OnMove(this, (int)(-options.MotionSpeed), (int)options.MotionSpeed));
        }

        void TurnRight()
        {
            _eventsPort.Post(new OnMove(this, (int)options.MotionSpeed, (int)(-options.MotionSpeed)));
        }

        void Stop()
        {
            _eventsPort.Post(new OnMove(this, 0, 0));
        }

        #endregion

    }

    #region Drive Control Event PortSet and Messages

    // This is the PortSet for the different types of messages from the WinForm
    public class DriveControlEvents
        : PortSet<
            OnLoad,             // Generic FormEvent
            OnClosed,           // Generic FormEvent
            OnConnect,
            OnMove,
            OnMotionCommand
        >
    {
    }

    // This is the base class for all Drive Control event messages,
    // i.e. messages that are specific to this form
    public class DriveControlEvent : FormEvent
    {
        public DriveControlEvent(DriveControl form) :
            base((Form) form)
        {
        }
    }

    /// <summary>
    /// On Move - Arrow key has been pressed
    /// </summary>
    // NOTE: This message type is also used by the WebCam Form
    public class OnMove : FormEvent
    {
        int _left;

        public int Left
        {
            get { return _left; }
            set { _left = value; }
        }

        int _right;

        public int Right
        {
            get { return _right; }
            set { _right = value; }
        }

        public OnMove(Form form, int left, int right)
            : base(form)
        {
            _left = left;
            _right = right;
        }
    }

    /// <summary>
    /// On Connect - Connect button has been pressed
    /// </summary>
    public class OnConnect : DriveControlEvent
    {
        string _service;

        public string Service
        {
            get { return _service; }
            set { _service = value; }
        }

        public OnConnect(DriveControl form, string service)
            : base(form)
        {
            _service = service;
        }
    }


    public enum MOTION_COMMANDS
    {
        Stop,
        Rotate,
        Translate
    }

    public class OnMotionCommand : DriveControlEvent
    {
        MOTION_COMMANDS _cmd;

        public MOTION_COMMANDS Command
        {
            get { return _cmd; }
            set { _cmd = value; }
        }

        double _param;

        public double Parameter
        {
            get { return _param; }
            set { _param = value; }
        }

        double _power;

        public double Power
        {
            get { return _power; }
            set { _power = value; }
        }

        public OnMotionCommand(DriveControl form, MOTION_COMMANDS command, double parameter, double power)
            : base(form)
        {
            _cmd = command;
            _param = parameter;
            _power = power;
        }
    }

    #endregion

}